
/*
 * foc.c
 *
 *  Created on: 2022.4.15
 *      Author: huan
 */
#include "svpwm.h"
#include "pwm_out.h"
#include "math.h"

#define _2_SQRT3    1.15470053838
#define _SQRT3      1.73205080757
#define _1_SQRT3    0.57735026919
#define _SQRT3_2    0.86602540378
#define _SQRT2      1.41421356237
#define _120_D2R    2.09439510239
#define _PI         3.14159265359
#define _PI_2       1.57079632679
#define _PI_3       1.04719755120
#define _2PI        6.28318530718
#define _3PI_2      4.71238898038
#define _PI_6       0.52359877559
#define _RAD        0.01745329252
#define Udc         12
#define PWM_Period  (100000000/PWM_FREQUENCY/2-1)

/* normalizing radian angle to [0,2PI] */
static float normalizeAngle(float angle)
{
    float a = fmod(angle, _2PI);
    return a >= 0 ? a : (a + _2PI);
}
/***************************************************************************/

void SVPWM(float Uq, float Ud, float angle_E)
{
    float Uout, remain;
    unsigned long int sector;
    float T0,T1,T2;
    float Ta,Tb,Tc;

    angle_E = angle_E*_RAD;             //convert to rad

    if(Ud == 0)Ud = 0.00000001;
    Uout = sqrt(Ud*Ud + Uq*Uq) / Udc;
    angle_E = normalizeAngle(angle_E + atan2(Uq , Ud));

    if(Uout >  1.0f)Uout =  1.0f;

    sector = (angle_E / _PI_3) + 1;     //determine sector
    remain = fmod(angle_E, _PI_3);      //determine relative angle
    T1 = sin(_PI_3 - remain) * Uout;    //U1
    T2 = sin(remain) * Uout;            //U2
    if(T1+T2 > 1)
    {
        T1 = T1/(T1+T2);
        T2 = T2/(T1+T2);
    }
    T0 = 1 - T1 - T2;

    // calculate the duty cycles(times)
    switch(sector)
    {
        case 1:
            Ta = T1 + T2 + T0/2;//U1
            Tb = T2 + T0/2;//U2
            Tc = T0/2;
            break;
        case 2:
            Ta = T1 +  T0/2;
            Tb = T1 + T2 + T0/2;
            Tc = T0/2;
            break;
        case 3:
            Ta = T0/2;
            Tb = T1 + T2 + T0/2;
            Tc = T2 + T0/2;
            break;
        case 4:
            Ta = T0/2;
            Tb = T1+ T0/2;
            Tc = T1 + T2 + T0/2;
            break;
        case 5:
            Ta = T2 + T0/2;
            Tb = T0/2;
            Tc = T1 + T2 + T0/2;
            break;
        case 6:
            Ta = T1 + T2 + T0/2;
            Tb = T0/2;
            Tc = T1 + T0/2;
            break;
        default:  // possible error state
            Ta = 0;
            Tb = 0;
            Tc = 0;
    }

    static unsigned short int T[3];
    T[0] = Ta * PWM_Period;
    T[1] = Tb * PWM_Period;
    T[2] = Tc * PWM_Period;

    Set_pwm(T);
    // 更新
}

